package pathfinder;

import java.awt.Color;
import java.awt.Graphics;
import java.util.ArrayList;

public class PointRobot
{
    private final int radius = 4;
    private final Color robotColor = Color.black;
    private final Color pathColor = Color.red;
    
    private Point start;
    private Point loc;
    private Point end;
    private ArrayList<Point> path;
    
    //For animation only
    private int curr_dest;
    
    PointRobot()
    {
        loc = new Point(0,0);
        start = new Point(0,0);
        end = new Point(0,0);
        path = new ArrayList<>();
        curr_dest = 0;
    }
    
    PointRobot(Point start, Point end)
    {
        loc = start.clone();
        this.start = start.clone();
        this.end = end.clone();
        path = new ArrayList<>();
        curr_dest = 0;
    }
    
    public void calculate(ArrayList<Block> blocks)
    {
        addPathPoint(start);
        //if (start.x==end.x)
        //{
            for(int i=0; i<blocks.size(); i++)
            {
                Block b = blocks.get(i);
                System.out.println(b.getTL());
                System.out.println(b.getTR());
                System.out.println(start);
                if (b.getTL().x < start.x && b.getTR().x > start.x 
                        && b.getTL().y>start.y)
                {
                    System.out.println("I am here");
                    addPathPoint(new Point(start.x-10,b.getTL().y));
                    addPathPoint(new Point(b.getTL().x-10,b.getTL().y));
                    addPathPoint(new Point(b.getBL().x-10,b.getBL().y));
                }
            }
       // }
        addPathPoint(end);
    }
    
    public Point getStart()
    {
        return start;
    }
    public Point getEnd()
    {
        return end;
    }
    public Point getLoc()
    {
        return loc;
    }
    public void addPathPoint(Point p)
    {
        path.add(p);
    }
    public Point getPathPoint(int index)
    {
        if(index < path.size())
        {
            return path.get(index);
        }
        else
        {
            return new Point(-1,-1);
        }
    }
    
    public void move(double speed)
    {
        double dir = World.direction(loc, path.get(curr_dest));
        double dx = speed * Math.cos(dir);
        double dy = speed * Math.sin(dir);
        if(loc.x < path.get(curr_dest).x && dx < 0)
        {
            dx = -dx;
        }
        else if(loc.x > path.get(curr_dest).x && dx > 0)
        {
            dx = -dx;
        }
        if(loc.y < path.get(curr_dest).y && dy < 0)
        {
            dy = -dy;
        }
        else if(loc.y > path.get(curr_dest).y && dy > 0)
        {
            dy = -dy;
        }
        Point new_loc = new Point(loc.x + dx,loc.y + dy);
        if(World.dist(loc, path.get(curr_dest)) <= World.dist(new_loc, path.get(curr_dest)))
        {
            loc = path.get(curr_dest).clone();

            if(curr_dest < path.size() - 1)
            {
                curr_dest++;
            }
        }
        else
        {
            loc = new_loc;
        }
    }
    
    public void draw(Graphics g)
    {
        //Draw the path
        g.setColor(pathColor);
        for(int k = 1; k<path.size(); k++)
        {
            g.drawLine((int) path.get(k-1).x, (int) path.get(k-1).y, (int) path.get(k).x, (int) path.get(k).y);
        }
        
        //Draw the robot
        g.setColor(robotColor);
        g.fillOval((int) loc.x - radius, (int) loc.y - radius, 2*radius, 2*radius);
    }
    public void clearPath()
    {
        path.clear();
    }
}
